//
// Created by ZhaoXiaoFei on 2022/8/18.
//

#include "model/init.hpp"

Initializer::Initializer(int num, State* state_){
    needImuNum = needGpsNum = needMagNum = num;
    state = state_;
}
void Initializer::Imu_initialize(const ImuData& imu_data){
    if(!isRecvFirstGps){
        return;
    }
    imu_buffer_.push_back(imu_data);
    if (imu_buffer_.size() > needImuNum){
        imu_buffer_.pop_front();
        isImuInit = true;
    }
}
void Initializer::Gps_initialize(const GpsPositionData& gps_data){
    isRecvFirstGps = true;
    if(isGpsInit){
        return;
    }
    if(gps_buffer_.size() < needGpsNum){
        gps_buffer_.push_back(gps_data);
    }
    else{
        Eigen::Vector3d meanGps{0,0,0};
        for(auto& it : gps_buffer_){
            meanGps += it.lla;
        }
        meanGps /= gps_buffer_.size();
        state->initLLA = meanGps;
        LOG(INFO) << "init position: " << meanGps(0) << " " << meanGps(1) << " " << meanGps(2) <<std::endl;
        isGpsInit = true;
    }
}
void Initializer::Mag_initialize(const MagData& mag_data){
    if(!isRecvFirstGps || isMagInit){
        return;
    }
    if(mag_buffer_.size() < needMagNum){
        mag_buffer_.push_back(mag_data);
    }
    else if(isImuInit && mag_buffer_.size() >= needMagNum){

        Eigen::Vector3d meanImu = Eigen::Vector3d::Zero();
        for (auto& it : imu_buffer_){
            meanImu += it.acc;
        }
        meanImu /= imu_buffer_.size();


        Eigen::Vector3d meanMag = Eigen::Vector3d::Zero();
        for (auto& it : mag_buffer_){
            meanMag += it.mag;
        }
        meanMag /= mag_buffer_.size();

        Eigen::Matrix3d R_IG;
        Eigen::Vector3d x_axis, y_axis, z_axis;

        z_axis = meanImu.normalized();
        x_axis = meanMag.cross(z_axis).normalized();
        y_axis = z_axis.cross(x_axis);

        R_IG.block<3, 1>(0, 0) = x_axis;
        R_IG.block<3, 1>(0, 1) = y_axis;
        R_IG.block<3, 1>(0, 2) = z_axis;

        state->pose_.block<3, 3>(0, 0) = R_IG.transpose();
        Eigen::Quaterniond q(R_IG.transpose());
        LOG(INFO) << "INIT Q.w: " << q.w() << " Q.x: " << q.x() << " Q.y: "  << q.y() << " Q.z: "  << q.z() << std::endl;

        state->imuData = imu_buffer_.back();
//            LOG(INFO) << "-------------------"<< std::endl;
//            LOG(INFO) << state->P_ << std::endl;
        isMagInit = true;
    }
}

bool Initializer::isInit(){
    isESKFInit = isImuInit && isGpsInit && isMagInit;
    return isESKFInit;
}
